function [ resstart resend ] = Collision( cylinder_starts,cylinder_ends,cylinder_rads,refstart,refend )
%COLLISION Summary of this function goes here
%   Detailed explanation goes here
crossps=[];
dists=[];
axisps=[];
for i=1:size(cylinder_starts,1)
    [dist cyp refp]=distBW2lines([cylinder_starts(i,:);cylinder_ends(i,:)],[refstart;refend]);
    if dist<cylinder_rads(i)
        norvec=refp-cyp;
        if all(norvec==0)
            crossp=(refstart-refend)/norm(refstart-refend)*cylinder_rads(i)+cyp;
            crossps=[crossps;crossp];
            dists=[dists;norm(crossp-refstart)];
            axisps=[axisps;cyp];
        else
            theta=acos(norm(norvec)/cylinder_rads(i));
            pv_rot=rodrigues_rot(norvec,cylinder_starts(i,:)-cylinder_ends(i,:),theta);
            tempstart=pv_rot/norm(pv_rot)*cylinder_rads(i)+cylinder_starts(i,:);
            tempend=pv_rot/norm(pv_rot)*cylinder_rads(i)+cylinder_ends(i,:);
            [~,crossp,~]=distBW2lines([tempstart;tempend],[refstart;refend]);
            if norm(crossp-tempstart)+norm(crossp-tempend)==norm(tempstart-tempend) ...
                    && norm(crossp-refstart)+norm(crossp-refend)==norm(refstart-refend)
                if crossp~=refstart
                    crossps=[crossps;crossp];
                    dists=[dists;norm(crossp-refstart)];
                    axisps=[axisps;crossp-pv_rot/norm(pv_rot)*cylinder_rads(i)];
                end
            end
            qv_rot=rodrigues_rot(norvec,cylinder_starts(i,:)-cylinder_ends(i,:),-theta);
            tempstart=qv_rot/norm(qv_rot)*cylinder_rads(i)+cylinder_starts(i,:);
            tempend=qv_rot/norm(qv_rot)*cylinder_rads(i)+cylinder_ends(i,:);
            [~,crossp,~]=distBW2lines([tempstart;tempend],[refstart;refend]);
            if norm(crossp-tempstart)+norm(crossp-tempend)==norm(tempstart-tempend) ...
                    && norm(crossp-refstart)+norm(crossp-refend)==norm(refstart-refend)
                if crossp~=refstart
                    crossps=[crossps;crossp];
                    dists=[dists;norm(crossp-refstart)];
                    axisps=[axisps;crossp-qv_rot/norm(qv_rot)*cylinder_rads(i)];
                end
            end
        end      
    end
end
if isempty(crossps)==0
    [~,index]=min(dists);
    resstart=crossps(index,:);
    tempvec=rodrigues_rot(refstart-crossps(index,:),crossps(index,:)-axisps(index,:),pi);
    resend=(norm(crossps(index,:)-refend))*tempvec/norm(tempvec)+crossps(index,:);
else
    resstart=refstart;
    resend=refend;
end

end

